#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/float64.hpp"

#include "hardware_driver/IMU.hpp"
#include "hardware_driver/MotorCan.hpp"

#include <memory>

class ChassisDriverMi9 : public rclcpp::Node
{
    using IMUMsg = sensor_msgs::msg::Imu;
    using WheelSpeedMsg = std_msgs::msg::Float64;
    using MotorForceMsg = std_msgs::msg::Float64;

  public:
    ChassisDriverMi9() : rclcpp::Node("ChassisDriverMi9")
    {
        leftWheelPub = create_publisher<WheelSpeedMsg>("/left_motor_speed", rclcpp::SensorDataQoS());
        rightWheelPub = create_publisher<WheelSpeedMsg>("/right_motor_speed", rclcpp::SensorDataQoS());
        imuPub = create_publisher<IMUMsg>("/IMU", rclcpp::SensorDataQoS());

        leftMotorForceSub =
            create_subscription<MotorForceMsg>("/left_motor_force", rclcpp::SystemDefaultsQoS().reliable(),
                                               [this](const MotorForceMsg &msg) { leftMotorForce = msg.data; });
        rightMotorForceSub =
            create_subscription<MotorForceMsg>("/right_motor_force", rclcpp::SystemDefaultsQoS().reliable(),
                                               [this](const MotorForceMsg &msg) { rightMotorForce = msg.data; });
    }

  private:
    rclcpp::Publisher<IMUMsg>::SharedPtr imuPub;
    rclcpp::Publisher<WheelSpeedMsg>::SharedPtr leftWheelPub;
    rclcpp::Publisher<WheelSpeedMsg>::SharedPtr rightWheelPub;
    rclcpp::Subscription<MotorForceMsg>::SharedPtr leftMotorForceSub;
    rclcpp::Subscription<MotorForceMsg>::SharedPtr rightMotorForceSub;

    HardWareDriver::MotorCan leftMotor;
    HardWareDriver::MotorCan rightMotor;
    HardWareDriver::IMU imu;

    double leftMotorForce;
    double rightMotorForce;

    void InitDevice()
    {
        auto server = std::make_shared<HardWareDriver::CanServer>();
        server->StartServer("/can0");
        HardWareDriver::MotorCan::MotorConfig config;
        config.address = 0;
        config.poleCount = 10;
        leftMotor.InitDevice(server, config);
        config.address = 1;
        config.poleCount = 10;
        rightMotor.InitDevice(server, config);
    }

    void OnStep()
    {
        WheelSpeedMsg leftSpeedMsg;
        leftSpeedMsg.data = leftMotor.GetRPM() / 60 * 2 * M_PI;
        leftWheelPub->publish(leftSpeedMsg);
        WheelSpeedMsg rightSpeedMsg;
        rightSpeedMsg.data = rightMotor.GetRPM() / 60 * 2 * M_PI;
        rightWheelPub->publish(rightSpeedMsg);
        RCLCPP_INFO(rclcpp::get_logger("SimDriverMi9"), "Speed is %f:%f", leftSpeedMsg.data, rightSpeedMsg.data);

        leftMotor.SetCurrent(leftMotorForce);
        rightMotor.SetCurrent(rightMotorForce);

        const double *quaternion = 0; // wb_inertial_unit_get_quaternion(IMUAngle);
        const double *gyro = 0;       // wb_gyro_get_values(IMUGyro);
        const double *acc = 0;        // wb_accelerometer_get_values(IMUAccelerometer);
        IMUMsg ImuMsg;
        ImuMsg.header.stamp = rclcpp::Clock().now();
        ImuMsg.header.frame_id = "IMU_frame";
        ImuMsg.orientation.x = quaternion[0];
        ImuMsg.orientation.y = quaternion[1];
        ImuMsg.orientation.z = quaternion[2];
        ImuMsg.orientation.w = quaternion[3];
        ImuMsg.angular_velocity.x = gyro[0];
        ImuMsg.angular_velocity.y = gyro[1];
        ImuMsg.angular_velocity.z = gyro[2];
        ImuMsg.linear_acceleration.x = acc[0];
        ImuMsg.linear_acceleration.y = acc[1];
        ImuMsg.linear_acceleration.z = acc[2];
        imuPub->publish(ImuMsg);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ChassisDriverMi9>());
    rclcpp::shutdown();
    return 0;
}